package com.hitqz.robot.driver.schedule;

import cn.hutool.extra.spring.SpringUtil;
import com.hitqz.robot.api.business.event.WheelOnlineEvent;
import com.hitqz.robot.api.common.dto.robot.RobotOnlineEventDto;
import com.hitqz.robot.api.common.entity.RobotMap;
import com.hitqz.robot.api.common.entity.RobotMapPos;
import com.hitqz.robot.driver.dto.LocationDto;
import com.hitqz.robot.driver.dto.PoseDto;
import com.hitqz.robot.driver.util.WheelCacheUtil;
import com.hitqz.robot.driver.util.WheelToolkit;
import lombok.extern.slf4j.Slf4j;
import org.springframework.scheduling.annotation.Scheduled;
import org.springframework.stereotype.Component;

import java.math.BigDecimal;
import java.math.RoundingMode;
import java.util.List;
import java.util.Objects;
import java.util.Optional;

@Component
@Slf4j
public class WheelSchedule {

    private boolean isOnlineLast;


    private static boolean lastSpeedCtrl = false;

    //定时检测心跳
    @Scheduled(fixedRate = 5000)
    public void heartBeatCheck() {
        long lastHeartBeatTimeStamp = Optional.ofNullable((Long) WheelCacheUtil.get(WheelCacheUtil.KEY_HEART_BEAT)).orElse(0L);
        long currentTimeStamp = System.currentTimeMillis();
        boolean isOnlineCurrent = currentTimeStamp - lastHeartBeatTimeStamp <= 10000;
        if (isOnlineCurrent!=isOnlineLast){
            int onlineStatus = isOnlineCurrent ? 1 : 0;
            //发布修改在线状态事件
            RobotOnlineEventDto dto = new RobotOnlineEventDto();
            dto.setOnlineStatus(onlineStatus);
            SpringUtil.getApplicationContext().publishEvent(dto);
            //发布上下线业务事件
            //定位关 或者 无位置上传 都是重启的表现
            PoseDto currentPose =(PoseDto) WheelCacheUtil.get(WheelCacheUtil.KEY_CURR_POSE);
            Integer locateEnable =(Integer) WheelCacheUtil.get(WheelCacheUtil.KEY_LOC_ENABLE_STATUS);
            boolean hasPos = currentPose != null && !Objects.equals(locateEnable, 0);
            SpringUtil.getApplicationContext().publishEvent(WheelOnlineEvent
                    .builder()
                    .onlineStatus(onlineStatus)
                    .hasPos(hasPos)
                    .build());
        }
        isOnlineLast = isOnlineCurrent;
    }


    /**
     * 监听web方向控制按钮
     */
    @Scheduled(fixedRate = 500)
    private void setRobotStop()  {
        boolean currentSpeedFlag = WheelCacheUtil.getSpeedFlag();
        if (!Objects.equals(lastSpeedCtrl, currentSpeedFlag)){
            if (!currentSpeedFlag) {
                // 停止
                WheelToolkit.setSpeed(0d,0d,0d,0d,0d,0d);
                log.info("[wheel]stop move ");
            }else {
                // 开始控制
                Integer navMode = WheelToolkit.getNavMode();
                // 建图模式
                List<Integer> buildMapMode = List.of(0, 1, 2, 7);
                if (!buildMapMode.contains(navMode)){
                    //不是建图模式下，要切换空闲模式
                    //避免这类情况:巡线等模式到目标点后会导航会定时下发速度0，如果这个时候上位机再进行速度控制，就会一顿一顿的
                    WheelToolkit.setNavMode(0);
                    log.info("[wheel]start speed control,change nav mode -> free");
                }
            }
        }
        lastSpeedCtrl = currentSpeedFlag;
    }


    @Scheduled(fixedRate = 1000)
    public void checkLocation(){
        if (WheelToolkit.posList == null || WheelToolkit.posList.isEmpty()){
            return;
        }
        PoseDto currentPose =(PoseDto) WheelCacheUtil.get(WheelCacheUtil.KEY_CURR_POSE);
        if (currentPose == null){
            return;
        }
        //计算最近点
        double minDistance = Double.MAX_VALUE;
        RobotMapPos nearestPos = null;
        for (RobotMapPos pos : WheelToolkit.posList){
            double distance = Math.sqrt(Math.pow(pos.getPosX() - currentPose.getX(), 2) + Math.pow(pos.getPosY() - currentPose.getY(), 2));
            if (distance < minDistance){
                minDistance = distance;
                nearestPos = pos;
            }
        }
        LocationDto locationDto = new LocationDto();
        locationDto.setCurrentPosCode(Optional.ofNullable(nearestPos).map(RobotMapPos::getCode).orElse(""));
        locationDto.setReach(minDistance < 0.1);
        //坐标转换
        PoseDto pixelPos = getRobotPos(currentPose);
        locationDto.setPixelX((int) pixelPos.getX());
        locationDto.setPixelY((int) pixelPos.getY());
        locationDto.setPixelAngle(pixelPos.getYaw());
        WheelCacheUtil.put(WheelCacheUtil.KEY_POSITION_STATUS, locationDto);
    }



    private PoseDto getRobotPos(PoseDto robotPoint) {
        RobotMap map = WheelToolkit.robotMap;
        if (map!=null && !Objects.equals(map.getType(),2)) {
            double dx = robotPoint.getX() - map.getMapOriginx();
            double dy = robotPoint.getY() - map.getMapOriginy();
            //激光雷达安装位置偏移角
            double theta = 0.0f;
            double cosTheta = Math.cos(theta);
            double sinTheta = Math.sin(theta);
            double resolution = map.getMapResolution();
            float yaw = BigDecimal.valueOf(robotPoint.getYaw() - theta).setScale(3, RoundingMode.HALF_UP).floatValue();
            float x = BigDecimal.valueOf((dx * cosTheta + dy * sinTheta) / resolution).setScale(3, RoundingMode.HALF_UP).floatValue();
            float y = BigDecimal.valueOf((dx * sinTheta - dy * cosTheta) / resolution).setScale(3, RoundingMode.HALF_UP).floatValue();
            return new PoseDto(x, y, 0f, yaw);
        }
        return robotPoint;
    }








}
